# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/02.CarRobot")
import inspect
from BwSensorLib import BwSensorLib
from CarRobot import CarRobot
from ArmRobot import ArmRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 
import ipdb
msg = """
请选择轮子模式 :
2 : 两轮
3 : 三轮
4 : 四向轮
5 : 四麦克朗姆轮
6 : 四并橡胶轮
CTRL-C to quit
"""

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"

class CarArmClass:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("serial")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 实例化carRobot类，并将mlib作为参数传入
        self.carRobot = CarRobot(self.mlib)
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 根据模块连接位置，自动加载ID配置,print_flag设为True，可打印打印当前模块信息
        # ipdb.set_trace()
        self.robotlib.module_ids = self.robotlib.scanNConfigCarIds(print_flag=True)
        # 配置轮式模式 2:两轮   3:三轮   4:四向轮  5:四麦克朗姆轮  6:四并橡胶轮
        print(msg)
        self.wheel_mode = BwHelpLib.inputNum()
        
        # 配置机器人（包括ID位置与速度模式）
        # 配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
        self.carRobot.bindIds(self.robotlib.module_ids, self.wheel_mode)
        # 设置小车速度
        self.speed = 50
        self.carRobot.update_speed(self.speed)

        # 实例化操作臂类
        self.armlib = ArmRobot(self.mlib)
        # 将模块ID与操作臂构型绑定
        self.armlib.bindIds(self.module_ids)        
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 机器人回到HOME位置
        self.armlib.goHome()
        # 更新当前操作臂的关节角状态
        self.armlib.updateAngles()

    #运行走四方案例
    # 流水灯模式 1:右转  2：左转 3：报警闪光  5：流水灯（初始化）
    def runDemo(self):
      waitSeconds = 2
      self.armlib.pickUpSample()
      self.carRobot.goForward()
      time.sleep(waitSeconds)
      self.carRobot.goBack()
      time.sleep(waitSeconds)
      self.carRobot.turnLeft()
      time.sleep(1)
      self.carRobot.turnRight()
      time.sleep(1)
      self.carRobot.stop()

      if(carRobot.Mode==5):
          carRobot.runLeft()
          time.sleep(waitSeconds)
          carRobot.runRight()
          time.sleep(waitSeconds)
          carRobot.stop()
      self.armlib.putDownSample()


if __name__ == "__main__":
    # 实例化避障小车类
    car_arm = CarArmClass()    
    try:
        # 执行小车走正方形功能3次（走3个正方形路线）
        for i in range(3):
            car_arm.runDemo()
    except KeyboardInterrupt:
        car_arm.carRobot.stop() # 小车停止运动
        car_arm.mlib.SetLEDBel(3) # 设置报警灯状态
        print(" Program closed! ")
    pass
 

